/**
  ******************************************************************************
  * @file     : chassis.cpp
  * @brief    : 底盘管理源文件
  ******************************************************************************
  * @attention
  * 对于底盘轮子的can ID:  前右 1 ,前左 2 ,后左 3 ,后右 4
  *
  *
  *******************************************************************************/
#include "chassis.h"
#include "can_motor_class.h"
#include "pid.h"

/*  外部变量链接区 start  */
extern DJI_3508   m_0x201_can1;       //底盘3508
extern DJI_3508   m_0x202_can1;       //底盘3508
extern DJI_3508   m_0x203_can1;       //底盘3508
extern DJI_3508   m_0x204_can1;       //底盘3508
extern DJI_3508   m_0x205_can1;       //防守3508

extern DJI_3508   m_0x201_can2;       //运球3508
extern DJI_3508   m_0x202_can2;       //运球3508
extern DJI_3508   m_0x203_can2;       //投篮3508
extern DJI_3508   m_0x204_can2;       //投篮3508
extern DJI_2006   m_0x205_can2;       //投篮2006
/*  外部变量链接区  end   */

/*   底盘命名空间   */
namespace chassis
{
    inline void get_motor_speed();
    void control();
    void solve();
    void inv_solve(float& fr, float& fl, float& bl, float& br);

    constexpr float  COS45          = 0.707106f  ;     /*  cos45°  */
    constexpr float  WHEEL_R        = 0.08f      ;     /*  轮子半径，单位 m  */
    constexpr float  L              = 0.3f       ;     /*  车体中心到轮心的水平距离  */
    constexpr float  pi             = 3.1415926f ;     /*  pi  */
    constexpr float  RPM_to_RPS     = 60.0f      ;     /*  rpm和rps转换  */
    constexpr float  reduction_rato = 19.203f    ;     /*  3508减速比    */

    float rps_front_right  =  0.0f;             //轮毂实际转速 rps
    float rps_front_left   =  0.0f;
    float rps_back_right   =  0.0f;
    float rps_back_left    =  0.0f;

    float Vx_get = 0.0f;                 //x轴实际速度    x轴正方向为右     m/s
    float Vy_get = 0.0f;                 //y轴实际速度    y轴正方向为前     m/s
    float Wz_get = 0.0f;                 //z轴实际角速度  右手定则为正      rps

    float Vx_target = 0.0f;             //x轴期望速度    x轴正方向为右     m/s
    float Vy_target = 0.0f;             //y轴期望速度    y轴正方向为前     m/s
    float Wz_target = 0.0f;             //z轴实际角速度  右手定则为正      rps

    pid_t angular_pid;                  //用于平动时修正底盘的角速度
}


/**
 * @brief 底盘正解算
 */
void chassis::solve()
{
    const float K_linear =      WHEEL_R * 2 * pi * COS45 / 4;            //线速度系数
    const float K_rotation =    WHEEL_R / (4 * L * COS45);             //角速度系数
    //计算x轴实际速度
    Vx_get = K_linear *
            (-rps_front_right - rps_front_left + rps_back_left + rps_back_right);
    //计算y轴实际速度
    Vy_get = K_linear *
            (rps_front_right - rps_front_left - rps_back_left + rps_back_right);
    //计算z轴实际速度
    Wz_get = K_rotation *
            (rps_front_right + rps_front_left + rps_back_left + rps_back_right);
}

/**
 * @brief 底盘逆解算
 * @param[out]  fr  前右      注意此处单位顺便转为rpm
 * @param[out]  fl  前左      注意此处单位顺便转为rpm
 * @param[out]  bl  后左      注意此处单位顺便转为rpm
 * @param[out]  br  后右      注意此处单位顺便转为rpm
 */
void chassis::inv_solve(float& fr, float& fl, float& bl, float& br)
{
    const float K_linear =    1.0f / (WHEEL_R * 2 * pi * COS45);    // 线速度系数
    const float K_rotation =  L / (WHEEL_R * 2 * pi * COS45);     // 角速度系数

    // 计算四个轮子的目标转速
    fr  =   K_linear * (-Vx_target + Vy_target) + K_rotation * Wz_target;
    fl  =   K_linear * (-Vx_target - Vy_target) - K_rotation * Wz_target;
    bl  =   K_linear * ( Vx_target - Vy_target) + K_rotation * Wz_target;
    br  =   K_linear * ( Vx_target + Vy_target) - K_rotation * Wz_target;
    fr *=   RPM_to_RPS;
    fl *=   RPM_to_RPS;
    bl *=   RPM_to_RPS;
    br *=   RPM_to_RPS;
}

/**
 * @brief 获取底盘电机的转速,rpm转rps,并且算上减速比
 */
inline void chassis::get_motor_speed()
{
    //rpm转rps,并且算上减速比
    rps_front_right  =  m_0x201_can1.speed_rpm_now / (RPM_to_RPS * reduction_rato) ;
    rps_front_left   =  m_0x202_can1.speed_rpm_now / (RPM_to_RPS * reduction_rato) ;
    rps_back_right   =  m_0x203_can1.speed_rpm_now / (RPM_to_RPS * reduction_rato) ;
    rps_back_left    =  m_0x204_can1.speed_rpm_now / (RPM_to_RPS * reduction_rato) ;
}

/**
 * @brief 底盘控制函数,在底盘任务中被循环调用
 */
void chassis::control(float vx_set, float vy_set, float wz_set)
{
    //先设置底盘期望速度
    chassis::Vx_target = vx_set;
    chassis::Vy_target = vy_set;
    chassis::Wz_target = wz_set;

    //底盘平动,(由于滤成四方向,所以x和y必定有一个是0)
    if( ( chassis::Vx_target == 0 || chassis::Vy_target == 0) &&  chassis::Wz_target == 0 )
    {
        //获取底盘电机的转速
        get_motor_speed();
        //运动学正解算得到当前速度,角速度
        solve();
        //给期望角速度加上一个pi控制器
        chassis::Wz_target += chassis::angular_pid.pid_calc(Wz_get,0);
    }else{}//不是平动就什么都不干

    //进行逆解算
    float fr, fl, bl, br;
    inv_solve(fr,fl,bl,br);

    //设置对应电机转速
    m_0x201_can1.set_speed(fr);
    m_0x202_can1.set_speed(fl);
    m_0x203_can1.set_speed(bl);
    m_0x204_can1.set_speed(br);
}

/**
 * @brief   全部电机停止输出
 */
void chassis::all_stop(void)
{
    m_0x201_can1.set_stop();
    m_0x202_can1.set_stop();
    m_0x203_can1.set_stop();
    m_0x204_can1.set_stop();
    m_0x205_can1.set_stop();
    m_0x201_can2.set_stop();
    m_0x202_can2.set_stop();
    m_0x203_can2.set_stop();
    m_0x204_can2.set_stop();
    m_0x205_can2.set_stop();
}